#include <fstream>
#include <string>
#include <vector>
#include <math.h>
#define PI 3.14159265
struct Distance{
    float distance;
    int used;
};

struct Edge{
    int v1;
    int v2;
    float distance;
};

struct Vertex2{
    int v;
    int used;
};

using namespace std;
vector<vector<float> > sites;
vector<vector<struct Distance> > distancemap;
vector<struct Edge > edges;
vector<vector<int> > clusters;

vector<vector<int> > mst_map;
vector<int>path;
vector<int>parents;
void tokenize(const string& str,
                      vector<string>& tokens,
                      const string& delimiters = " ")
{
    // Skip delimiters at beginning.
    string::size_type lastPos = str.find_first_not_of(delimiters, 0);
    // Find first "non-delimiter".
    string::size_type pos     = str.find_first_of(delimiters, lastPos);

    while (string::npos != pos || string::npos != lastPos)
    {
        // Found a token, add it to the vector.
        tokens.push_back(str.substr(lastPos, pos - lastPos));
        // Skip delimiters.  Note the "not_of"
        lastPos = str.find_first_not_of(delimiters, pos);
        // Find next "non-delimiter"
        pos = str.find_first_of(delimiters, lastPos);
    }
}

void parseLine(string line)
{
    /*Tokenize string*/
    vector<string> tokens;
    tokenize(line, tokens, " ");
    if (tokens.size() == 3) {
        //Format: 4592 6.1 5"
        vector<float> site(3);
        site[0] = atof(tokens[0].c_str());
        site[1] = atof(tokens[1].c_str());
        site[2] = atof(tokens[2].c_str());
        sites.push_back(site);
    }
    else {
        fprintf(stderr, "Skipping line: %s\n", line.c_str());
    }
}

void readFile()
{
    ifstream stream;
    string line;
    // Now read the tour sites file
    char * msFile = "hw4.tour";
    stream.open(msFile);
    if (!stream.is_open()) {
        fprintf(stderr, "Failed to open %s\n", msFile);
        exit(1);
    }

    while (!stream.eof()) {
        getline(stream, line);
        parseLine( line);
        //printf("%s\n",line.c_str());
    }
    stream.close();
}

float getDistance(vector<float> a, vector<float> b)
{
    return sqrt(pow(a[0]-b[0], 2) + pow(a[1]-b[1], 2) + pow(a[2]-b[2], 2)); 
}

struct Edge findMinimum()
{
    float min = 1000000.f;
    struct Edge edge;
    edge.v1 = -1;
    struct Distance minDistance;
    for (int a = 0; a < distancemap.size(); a++)
    {
        vector<struct Distance>distances = distancemap[a];
        for (int b = 0; b < distances.size(); b++)
        {
            struct Distance e = distances[b];
            //printf("distance = %f\n", 
            if (e.used == 0 && e.distance < min)
            {
                edge.v1 = a;
                edge.v2 = b;
                edge.distance = e.distance;
                min = e.distance;
                minDistance = e;
            }
        }
    }
    
    if (edge.v1 == -1)
    {
        printf("No Distances found\n");
    }
    else
    {
        printf("Found min: %d, %d, %f\n",edge.v1, edge.v2, edge.distance);
        distancemap[edge.v1][edge.v2].used = 1;
        distancemap[edge.v2][edge.v1].used = 1;
    }
    return edge;
}

/*bool formsCycle(struct Edge e)
{
    for (int a = 0; a < edges.size(); a++)
    {
        //is e.v1 present?
        if (edges[0].v1 == e.v1 || edges
    }
}*/

int getCluster(int v)
{
    bool found = false;
    for (int a = 0; a < clusters.size(); a++)
    {
        vector<int>cluster = clusters[a];
        for (int b = 0; b < cluster.size(); b++)
        {
            if (cluster[b] == v)
                return a;
        }
    }
    printf("Error in getCluster, cluster not found for vertex %d\n",v);
    return -1;
}

void mergeClusters(int c1, int c2)
{
    vector<int>cluster1 = clusters[c1];
    vector<int>cluster2 = clusters[c2];
    for (int a = 0; a < cluster2.size(); a++)
    {
        clusters[c1].push_back(cluster2[a]);
    }
    clusters[c2].clear();
}

void calcMST()
{
    for (int a = 0; a < sites.size(); a++)
    {
        vector<int> cluster;
        cluster.push_back(a);
        clusters.push_back(cluster);
    }
    
    //go through all the neighbors. it is a bunch of Distances.
    while(true)
    { 
        struct Edge edge = findMinimum();
        if (edge.v1 == -1)
            break;
         
        if (getCluster(edge.v1) != getCluster(edge.v2))
        {
            edges.push_back(edge);
            mergeClusters(getCluster(edge.v1), getCluster(edge.v2));
        }
    }
}

void constructMap()
{
    for (int a = 0; a < sites.size(); a++)
    {
        vector<struct Distance>distances;
        for (int b = 0; b  < sites.size(); b++)
        {
            struct Distance e;
            if (a == b)
            {
                e.distance = 1000000.;
                e.used = 1;
            }
            else
            {
                e.distance = getDistance(sites[a],sites[b]);
                e.used = 0;
                printf("Distance between %d and %d: %f\n",a,b,e.distance);
                
            }
            distances.push_back(e);
            
        }
        distancemap.push_back(distances);
    }
}

void printEdges()
{
    printf("MST:\n");
    for (int a = 0; a < edges.size(); a++)
    {
        struct Edge e = edges[a];
        printf("v1: %d v2: %d distance %f\n", e.v1, e.v2, e.distance);
    }
}

void constructMap2()
{
    for (int a = 0; a < sites.size();  a++)
    {
        vector<int> new_edges;
        for (int b = 0; b < edges.size(); b++)
        {
            struct Edge e = edges[b];
            if (e.v1 == a)
            {
                new_edges.push_back(e.v2);
            }
            else if (e.v2 == a)
            {
                new_edges.push_back(e.v1);
            }
        }
        mst_map.push_back(new_edges);
    }
}

int startingVertex()
{
    for (int a = 0; a < sites.size(); a++)
    {
        if (mst_map[a].size() == 1)
            return a;
    }
    printf("didnt find any vertices that had degree 1\n");
    return -1;
}

bool array_contains(vector<int> array, int b)
{
    for (int a = 0; a < array.size(); a++)
    {
        if (array[a] == b)
            return true;
    }
    return false;
}

vector<float> diff2(vector<float> A, vector<float>B)
{
    vector<float> C(2);
    C[0] = A[0] - B[0];
    C[1] = A[1] - B[1];
    return C;
}

vector<float> normalize2(float x, float y)
{
    float len = sqrt(pow(x,2) + pow(y,2));
    vector<float> rval(2);
    rval[0] = x/len;
    rval[1] = y/len;
    return rval;
}

float crossp2(vector<float>a, vector<float>b)
{
    
    float z = a[0]*b[1] - a[1]*b[0];
    return z;
}

float dotp2(vector<float>a, vector<float> b)
{
    return a[0]*b[0] + a[1]*b[1];
}

float toDegrees(float radians)
{
    return radians*180./PI;
}

float calcAngle(vector<float>a, vector<float>b)
{
    //printf("a: %f %f, b: %f %f\n",a[0],a[1],b[0],b[1]);
    vector<float>a2 = normalize2(a[0],a[1]);
    vector<float>b2 = normalize2(b[0],b[1]);
    float angle = toDegrees(acos(dotp2(a2,b2)));
    //printf("angle: %f\n", angle);
    float z = crossp2(a2,b2);
    //printf("z: %f\n", z);
    if (z < 0.) angle*=-1.;
    return angle;
}

/*
 * Returns the next vertex. 
 * -1 if no vertex found */
int getNextVertex(vector<float>curDir, int v)
{
    vector<int> v2s = mst_map[v];
    int vertex = -1;
    float minAngle = 181.;
    float cAngle;
    for (int a = 0; a < v2s.size(); a++)
        {
            //printf("v2s[a]: %d\n", v2s[a]);
            if (!array_contains(path, v2s[a]))
            {
                //printf("0.1 ");
                //begin test
                if (v == 5) printf("v2s[a]: %d\n", v2s[a]);
                vector<float> nextDir = diff2(sites[v2s[a]],sites[v]);
                //printf("0.2");
                
                cAngle = calcAngle(curDir, nextDir);
                if (v == 5)  printf("cAngle: %f\n, minAngle: %f\n", cAngle,minAngle);
                if (cAngle < minAngle)
                {
                    minAngle = cAngle;
                    vertex = v2s[a];
                }
                printf("");
            }
            //printf("2. v2s[a]: %d\n", v2s[a]);
        }
        
             //       printf("vertex: %i\n",vertex);
        return vertex;
}

void init_parent()
{
    for (int a = 0; a < sites.size(); a++)
    {
        parents.push_back(-1);
    }
}

void calcPath()
{
    int vertex = path[0];
    init_parent();
    vector<float>curDir(2);
    curDir[0] = 1;
    curDir[1] = 0;
    int next_vertex;
    while(true)
    {
        next_vertex = getNextVertex(curDir, vertex);
        //if it is negative 1, then step back and try agin
        while (next_vertex == -1)
        {
            int parent = parents[vertex];
           
            if (parent == -1)
            {
                //finished. 
                printf("Shouldn't be here\n");
            }
            curDir = diff2(sites[parent], sites[vertex]);
             vertex = parent;
            next_vertex = getNextVertex(curDir, parent);
        }
        path.push_back(next_vertex);
        if (path.size() == sites.size())
        {
            printf("done\n");
            break;
        }
        if (parents[next_vertex] == -1)
            parents[next_vertex] = vertex;
        curDir = diff2(sites[next_vertex],sites[vertex]);
        vertex = next_vertex;
        
        
    }
    
   /* while (true)
    {
    vector<int> v2s = mst_map[vertex];
    if (v2s.size() == 1)
    {
        curDir = diff3(sites[v2s[0]],sites[vertex]);
        if (contains(path,v2s[0]))
        {
            int prevVertex = 
            
        }
        else
        {
            vertex = v2s[0];
        }
        path.push_back(vertex);       
    }
    else{
        vertex = getNextVertex(curDir, vertex);
        if (vertex != -1)
            path.push_back(vertex);
        else
            
    }*/
}

void printPath()
{
    ofstream stream("optimized.tour");
    if(!stream)
    {
        printf("Can't open file.\n");
        exit(2);
    }
    
    printf("Begin final path\n");
    for (int a = 0; a < path.size(); a++)
    {
        printf("%d\n",path[a]);
        stream << sites[path[a]][0] << " " << sites[path[a]][1] << " " << sites[path[a]][2]<<endl;
    }
}
int main()
{
    readFile();
    constructMap();
    calcMST();
    printEdges();
    constructMap2();
    int a = startingVertex();
    path.push_back(a);
    calcPath();
    printPath();
    return 0;
}
